// Emm_V5.c
// Author: Franki
// Date: 2025-02-13
#include "Emm_V5.h"
#include"../BSP/DWT/DWT.h"
#include"../ACTION/Action/basis_Action.h"
#include "main.h"
#include "usart.h"


int A[6];
int B[6]={1,2,3,1,2,3};

/**
 * @brief       模拟PWM信号输出
 * @param       io:输出io steps:脉冲数 cnt:脉冲频率
 * @retval      无
 */
void Emm_PWM_OUT(gpio_Conf io,int32_t steps,int32_t cnt)    //steps 段数 角度    cnt 是周期
{
    if (cnt==0)
        return;
    if (cnt/2<836)
    {
        for (int32_t i = 0; i < steps; i++) {
            //GPIOA->BRR |=(1<<6);
            HAL_GPIO_WritePin(io.def,io.pin,GPIO_PIN_SET);
            dwt_delay_us(cnt/2);
            //GPIOA->BSRR |=(1<<6);
            HAL_GPIO_WritePin(io.def,io.pin,GPIO_PIN_RESET);
            dwt_delay_us(cnt/2);
        }
    } else{
        for (int32_t i = 0; i < steps; i++) {
            HAL_GPIO_WritePin(io.def,io.pin,GPIO_PIN_SET);

            for (int32_t j = 0; j < ((cnt/2)/836); j++) {
                dwt_delay_us(836);

            }
            dwt_delay_us(cnt/2-((cnt/2)/836));

            HAL_GPIO_WritePin(io.def,io.pin,GPIO_PIN_RESET);
            for (int32_t j = 0; j < ((cnt/2)/836); j++) {
                dwt_delay_us(836);
            }
            dwt_delay_us(cnt/2-((cnt/2)/836));
        }
    }
}

void Emm_PWM_remove_JJ(int32_t Z){
    int32_t distance_mm=Z;
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_RESET);//前进
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}

void Emm_PWM_remove_JJ_SET(int32_t Z){
    int32_t distance_mm=Z;
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_SET);//后退
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}

void Emm_PWM_remove_JJ_go(){
    int32_t distance_mm=920;
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_RESET);//前进
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}



void Emm_PWM_remove_JJ_spin(){//开始旋转 12.5
int32_t steps_needed =(int32_t)(440*3200/360.0);  // 计算需要的步数
int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_RESET);//前进
gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
Emm_PWM_OUT(motor, steps_needed, pwm_period);
}

void Emm_PWM_remove_JJ_spin_go(){//开始旋转 12.5
    int32_t steps_needed =(int32_t)(50*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_RESET);//前进
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}



void Emm_PWM_remove_JJ_distance(int32_t Z){//x为1到10
int32_t steps_needed =(int32_t)(Z*95*3200/360.0);  // 计算需要的步数
int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_RESET);//前进
gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
Emm_PWM_OUT(motor, steps_needed, pwm_period);}






void Emm_PWM_remove_JJ_back() {
    int32_t steps_needed = (int32_t) (920 * 3200 / 360.0);  // 计算需要的步数
    int32_t pwm_period = (60 * 1000000) / (80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port, JJ_DIR_Pin, GPIO_PIN_SET);//前进
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}
void Emm_PWM_remove_JJ_spin_go_back(){
    int32_t steps_needed =(int32_t)(835*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);     // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(JJ_DIR_GPIO_Port,JJ_DIR_Pin,GPIO_PIN_SET);//前进
    gpio_Conf motor = {JJ_STP_GPIO_Port, JJ_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}


 void  Emm_PWM_remove_KK_up(int32_t distance_mm)
 {
     int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数
     int32_t pwm_period = (60*1000000)/(65*STPES_PER_REVOLUTION);      // 计算 PWM 周期 (us)
     HAL_GPIO_WritePin(KK_DIR_GPIO_Port,KK_DIR_Pin,GPIO_PIN_SET);// 从轴看向电机 逆时针时针 向上
     gpio_Conf motor = {KK_STP_GPIO_Port, KK_STP_Pin};       // GPIO 配置
     Emm_PWM_OUT(motor, steps_needed, pwm_period);
 }

void  Emm_PWM_remove_KK_SET(int32_t distance_mm)
{
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);      // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(KK_DIR_GPIO_Port,KK_DIR_Pin,GPIO_PIN_SET);// 从轴看向电机 逆时针时针 向上
    gpio_Conf motor = {KK_STP_GPIO_Port, KK_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}


void Emm_PWM_remove_KK_down(int32_t distance_mm){
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数   835 到了四分之一
    int32_t pwm_period = (60*1000000)/(60*STPES_PER_REVOLUTION);      // 计算 PWM 周期 (us)
     HAL_GPIO_WritePin(KK_DIR_GPIO_Port,KK_DIR_Pin,GPIO_PIN_RESET);// 从轴看向电机 顺时针 向下
    gpio_Conf motor = {KK_STP_GPIO_Port, KK_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}





void Emm_PWM_remove_KK(int32_t distance_mm){
    int32_t steps_needed =(int32_t)(distance_mm*3200/360.0);  // 计算需要的步数  下降 835 到了四分之一
    int32_t pwm_period = (60*1000000)/(80*STPES_PER_REVOLUTION);      // 计算 PWM 周期 (us)
    HAL_GPIO_WritePin(KK_DIR_GPIO_Port,KK_DIR_Pin,GPIO_PIN_RESET);// 从轴看向电机 顺时针 向下
    gpio_Conf motor = {KK_STP_GPIO_Port, KK_STP_Pin};       // GPIO 配置
    Emm_PWM_OUT(motor, steps_needed, pwm_period);
}




void  start_scan(){
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    uint8_t message[] = "XL";
    uint16_t received_size = 0;

    catching_flip_watch();
    HAL_UART_Transmit(&huart1, message, sizeof(message), 10);
    HAL_Delay(3000);


    Emm_PWM_remove_KK_down(800);
    catching_flip_watch1();
    HAL_Delay(2000);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port,FLAG_OUT_Pin,GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port,FLAG_OUT_Pin,GPIO_PIN_RESET);
}



void  start(){


    Emm_PWM_remove_JJ_spin();
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port,FLAG_OUT_Pin,GPIO_PIN_RESET);
}


void start_Receive(){

    uint8_t message[] = "LIAO";
    int P=0;

    uint16_t received_size = 0;

    catching_flip_watch();
    HAL_UART_Transmit(&huart1, message, sizeof(message), HAL_MAX_DELAY);
    HAL_Delay(3000);
    // 发送消息

    while (P<6) {
        static uint8_t rx_buffer[1];

        received_size = HAL_UART_Receive(&huart1, rx_buffer, sizeof(rx_buffer), 10); // 设置超时时间为10ms

        if (received_size > 0) {
            // 处理接收到的数据
            if (rx_buffer[0] == '1' || rx_buffer[0] == '2' || rx_buffer[0] == '3') {
                HAL_Delay(100); //... 红1 蓝 2 绿3

                B[P] = rx_buffer[0] - '0';  // 把rx_buffer转换成整数存入A[i]
                HAL_UART_Transmit(&huart1, rx_buffer, sizeof(rx_buffer), 10);
                HAL_Delay(1000);
                P++;
            }
        }
    }
}


void start_catch(){

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    HAL_Delay(100);
    Emm_PWM_remove_JJ_spin_go();
    Emm_PWM_remove_KK_up(1100);
    uint8_t message[] = "LIAO";
    int i=0;
    uint16_t received_size = 0;
    catching_flip_watch();
    HAL_UART_Transmit(&huart1, message, sizeof(message), HAL_MAX_DELAY);
    HAL_Delay(3000);
    // 发送消息

    while (i<3)
    {
        uint8_t rx_buffer[1];

        received_size = HAL_UART_Receive(&huart1, rx_buffer, sizeof(rx_buffer), 10); // 设置超时时间为10ms

        if (received_size > 0) {
            // 处理接收到的数据
            if (rx_buffer[0] == '1' || rx_buffer[0] == '2' || rx_buffer[0] == '3') {
                HAL_Delay(100);//... 红1 蓝 2 绿3
                switch (i) {
                    case 0:
                        catching_flip();
                        catching_init();
                        Emm_PWM_remove_KK_down(650);
                        HAL_Delay(2000);
                        catching_catch();
                        HAL_Delay(1000);
                        catching_flip_watch1();
                        Emm_PWM_remove_KK_up(1100);
                        Turntable_spin_1();
                        catching_flip_init();
                        HAL_Delay(1000);
                        catching_init();
                        HAL_Delay(2000);
                        catching_flip();
                        catching_flip_watch();
                        HAL_Delay(3000);
                        break;

                    case 1:
                        catching_flip();
                        catching_init();
                        Emm_PWM_remove_KK_down(800);
                        HAL_Delay(2000);
                        catching_catch();
                        HAL_Delay(1000);
                        catching_flip_watch1();
                        Emm_PWM_remove_KK_up(1400);
                        catching_flip();
                        Turntable_spin_2();
                        catching_flip_init();
                        HAL_Delay(1000);
                        catching_init();
                        HAL_Delay(2000);
                        catching_flip();
                        catching_flip_watch();
                        HAL_Delay(3000);
                        break;
                    case 2:
                        catching_flip();
                        catching_init();
                        Emm_PWM_remove_KK_down(920);
                        HAL_Delay(2000);
                        catching_catch();
                        HAL_Delay(1000);
                        catching_flip_watch1();
                        Emm_PWM_remove_KK_up(1700);
                        catching_flip();
                        Turntable_spin_3();
                        catching_flip_init();
                        HAL_Delay(1000);
                        catching_init();
                        HAL_Delay(2000);
                        catching_flip();
                        break;
                    default:
                        break;


                }
                A[i] = rx_buffer[0] - '0';  // 把rx_buffer转换成整数存入A[i]
                HAL_UART_Transmit(&huart1, rx_buffer, sizeof(rx_buffer), 10);
                HAL_Delay(1000);
                i++;
            }
        }
    }



    Turntable_spin_1();
    Emm_PWM_remove_JJ_SET(350);
    HAL_Delay(100);


}


void start_catch_down_1() {

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[0] == 3) {
        Emm_PWM_remove_JJ(150);
        for (int j = 0; j < 3; j++) {
            if (B[0] == A[j]) {
                Turntable_spin(j);
                HAL_Delay(10);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                HAL_Delay(1000);
                catching_catch();
                HAL_Delay(1000);
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                HAL_Delay(1000);
                catching_init();
                HAL_Delay(1000);
                Emm_PWM_remove_KK_down(1600);
                HAL_Delay(1000);
                Emm_PWM_remove_JJ_SET(150);
            }
        }


    } else
        for (int j = 0; j < 3; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                HAL_Delay(1000);
                catching_catch();
                HAL_Delay(1000);
                catching_flip();
                HAL_Delay(1000);
                Emm_PWM_remove_KK_down(700);
                HAL_Delay(1000);
                catching_init(); HAL_Delay(1000);
                Emm_PWM_remove_KK_down(1600);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}

void start_catch_down_2(){



    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[1] == 3) {
        Emm_PWM_remove_JJ(50);
        for (int j = 0; j < 3; j++) {
            if (B[1] == A[j]) {
                Emm_PWM_remove_JJ_SET(150);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                HAL_Delay(1000);
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                HAL_Delay(1000);
                catching_init();
                HAL_Delay(1000);
                Emm_PWM_remove_KK_down(1600);
                HAL_Delay(1000);
                Emm_PWM_remove_JJ_SET(150);
            }
        }


    } else
        for (int j = 0; j < 3; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                HAL_Delay(1000);
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                HAL_Delay(1000);
                catching_init();
                Emm_PWM_remove_KK_down(1600);
                HAL_Delay(1000);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}




void start_catch_down_3() {


    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[2] == 3) {
        Emm_PWM_remove_JJ(150);
        for (int j = 0; j < 3; j++) {
            if (B[2] == A[j]) {
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(150);
            }
        }


    } else
        for (int j = 0; j < 3; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}
    void start_catch2() {


        while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
        HAL_Delay(100);
        Emm_PWM_remove_JJ_spin_go();
        Emm_PWM_remove_KK_up(1200);


        uint8_t message[] = "LIAO";
        int i = 3;

        uint16_t received_size = 0;

        catching_flip_watch();
        HAL_UART_Transmit(&huart1, message, sizeof(message), HAL_MAX_DELAY);
        HAL_Delay(3000);
        // 发送消息

        while (i < 6) {
            uint8_t rx_buffer[1];

            received_size = HAL_UART_Receive(&huart1, rx_buffer, sizeof(rx_buffer), 10); // 设置超时时间为10ms

            if (received_size > 0) {
                // 处理接收到的数据
                if (rx_buffer[0] == '1' || rx_buffer[0] == '2' || rx_buffer[0] == '3') {
                    HAL_Delay(100);//... 红1 蓝 2 绿3
                    switch (i) {
                        case 0:
                            catching_flip();
                            catching_init();
                            Emm_PWM_remove_KK_down(550);
                            HAL_Delay(2000);
                            catching_catch();
                            Emm_PWM_remove_KK_up(1100);
                            Turntable_spin_1();
                            catching_flip_init();
                            HAL_Delay(1000);
                            catching_init();
                            HAL_Delay(2000);
                            catching_flip();
                            catching_flip_watch();
                            HAL_Delay(3000);
                            break;

                        case 1:
                            catching_flip();
                            catching_init();
                            Emm_PWM_remove_KK_down(700);
                            HAL_Delay(2000);
                            catching_catch();
                            Emm_PWM_remove_KK_up(1400);
                            catching_flip();
                            Turntable_spin_2();
                            catching_flip_init();
                            HAL_Delay(1000);
                            catching_init();
                            HAL_Delay(2000);
                            catching_flip();
                            catching_flip_watch();
                            HAL_Delay(3000);
                            break;
                        case 2:
                            catching_flip();
                            catching_init();
                            Emm_PWM_remove_KK_down(920);
                            HAL_Delay(2000);
                            catching_catch();
                            Emm_PWM_remove_KK_up(1800);
                            catching_flip();
                            Turntable_spin_3();
                            catching_flip_init();
                            HAL_Delay(1000);
                            catching_init();
                            HAL_Delay(2000);
                            catching_flip();
                            break;
                        default:
                            break;


                    }
                    A[i] = rx_buffer[0] - '0';  // 把rx_buffer转换成整数存入A[i]
                    HAL_UART_Transmit(&huart1, rx_buffer, sizeof(rx_buffer), 10);
                    HAL_Delay(1000);
                    i++;
                }
            }


        }
    }





void start_catch_down_4(){


    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[3] == 3) {
        Emm_PWM_remove_JJ(50);
        for (int j = 3; j < 6; j++) {
            if (B[3] == A[j]) {
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(50);
            }
        }


    } else
        for (int j = 3; j < 6; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}




void start_catch_down_5(){



    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[4] == 3) {
        Emm_PWM_remove_JJ(50);
        for (int j = 3; j < 6; j++) {
            if (B[4] == A[j]) {
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(50);
            }
        }


    } else
        for (int j = 3; j < 6; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}


void start_catch_down_6(){



    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));


    if (B[5] == 3) {
        Emm_PWM_remove_JJ(50);
        for (int j = 3; j < 6; j++) {
            if (B[5] == A[j]) {
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(50);
            }
        }


    } else
        for (int j = 3; j < 6; j++) {

            {
                Emm_PWM_remove_JJ(170);
                Turntable_spin(j);
                Emm_PWM_remove_KK_up(200);
                catching_flip();
                catching_flip_init();
                catching_catch();
                catching_flip();
                Emm_PWM_remove_KK_down(700);
                catching_init();
                Emm_PWM_remove_KK_up(1600);
                Emm_PWM_remove_JJ_SET(170);

            }


            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
            HAL_Delay(100);
            HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);
        }

}



void start_catch_up_1(){    //先抓蓝

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(1);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(170);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//蓝放在1盘

}



void start_catch_up_2(){    //抓绿

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(50);
    Turntable_spin(2);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(50);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放在2盘

}


void start_catch_up_3(){    //抓红

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(3);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(170);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//红放在3盘

}


void start_catch_up_4(){    //抓蓝

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(1);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(10);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放在2盘

}




void start_catch_up_5(){    //抓绿

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(20);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(50);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放在2盘

}





void start_catch_up_6(){    //抓红

    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(3);
    Emm_PWM_remove_KK_down(900);
    catching_init();
    catching_catch();
    Emm_PWM_remove_KK_up(2000);
    catching_flip_init();
    HAL_Delay(1000);
    catching_init();
    Emm_PWM_remove_JJ_SET(50);
    catching_flip();
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放在2盘

}


void start_catch_down_F1(){   //放蓝
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(1);
   catching_flip_init();
   catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(900);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(2000);
    Emm_PWM_remove_JJ_SET(170);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放在2盘

}



void start_catch_down_F2(){   //放绿
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(50);
    Turntable_spin(2);
    catching_flip_init();
    catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(900);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(2000);
    Emm_PWM_remove_JJ_SET(50);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放

}

void start_catch_down_F3(){   //放红
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(3);
    catching_flip_init();
    catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(900);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(2000);
    Emm_PWM_remove_JJ_SET(170);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放

}

// Your C functions and implementation go here
void start_catch_down_F4(){   //放红
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(170);
    Turntable_spin(1);
    catching_flip_init();
    catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(200);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(700);
    Emm_PWM_remove_JJ_SET(70);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放

}


void start_catch_down_F5(){   //放红
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(50);
    Turntable_spin(2);
    catching_flip_init();
    catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(200);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(700);
    Emm_PWM_remove_JJ_SET(50);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放

}



void start_catch_down_F6(){   //放红
    while (!HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2));
    Emm_PWM_remove_JJ(70);
    Turntable_spin(3);
    catching_flip_init();
    catching_catch();
    HAL_Delay(1000);
    catching_flip();
    Emm_PWM_remove_KK_down(200);
    catching_init();
    HAL_Delay(1000);
    Emm_PWM_remove_KK_up(700);
    Emm_PWM_remove_JJ_SET(70);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_SET);
    HAL_Delay(100);
    HAL_GPIO_WritePin(FLAG_OUT_GPIO_Port, FLAG_OUT_Pin, GPIO_PIN_RESET);//绿放

}